'''
1、接受tessng向kafka发送的车辆数据,并进行展示

2、万集64线雷达,可以实时展示雷达的点云图像,并将进行保存

'''

import carla
import numpy as np
import time
from queue import Queue, Empty
from carla import Transform, Rotation, Location
from kafka import KafkaConsumer
import json
from pyproj import Proj
import cv2
import copy
import open3d as o3d
import os


# 经纬度转xy坐标，在carla中y坐标的值需要取反
rule = Proj("+proj=tmerc +lat_0=40.04752343063863 +lon_0=116.2872228823074 +k=1 +x_0=0 +y_0=0 +datum=WGS84")

# 将雷达的点云保存到队列
def sensor_callback(sensor_data, sensor_queue, sensor_name):
    sensor_queue.put((sensor_data.frame, sensor_data.timestamp, sensor_name, sensor_data))

# 时间格式化
def get_time_stamp(ct):
    local_time = time.localtime(ct)
    data_head = time.strftime("%Y-%m-%d %H-%M-%S", local_time).split(' ')[-1]
    data_secs = (ct - int(ct)) * 1000
    time_stamp = "%s-%03d" % (data_head, data_secs)
    return time_stamp

def _parse_lidar_cb(lidar_data):
    points = np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4'))
    points = copy.deepcopy(points)
    points = np.reshape(points, (int(points.shape[0] / 4), 4))
    return points

# 保存点云数据
def points2pcd(PCD_FILE_PATH, points):

    if os.path.exists(PCD_FILE_PATH):
        os.remove(PCD_FILE_PATH)
    # 写文件句柄
    handle = open(PCD_FILE_PATH, 'a')
    # 得到点云点数
    point_num = points.shape[0]
    # pcd头部（重要）
    handle.write(
        '# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z intensity\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1')
    string = '\nWIDTH ' + str(point_num)
    handle.write(string)
    handle.write('\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0')
    string = '\nPOINTS ' + str(point_num)
    handle.write(string)
    handle.write('\nDATA ascii')

    # 依次写入点
    for i in range(point_num):
        string = '\n' + str(points[i, 0]) + ' ' + str(points[i, 1]) + ' ' + str(points[i, 2]) + ' ' + str(points[i, 3])
        handle.write(string)
    handle.close()

def main():
  
    try:
                     
    #  ======================================跟服务器实现连接=============================================
        client = carla.Client('localhost', 2000)
        client.set_timeout(10.0)
                 
    #  ======================================使用自定义地图=============================================
        world = client.load_world('shangdiwr')

    # =============================天气=================================

        # 控制世界的天气和时间（太阳的位置） 万里无云，没有降雨，太阳的角度为90
        weather = carla.WeatherParameters(
            cloudiness=0.0,  # 0-100  0 是晴朗的天空，100 是完全阴天
            precipitation=0.0,  # 0 表示没有下雨，100 表示大雨
            precipitation_deposits=0.0, # 0 表示道路上没有水坑，100 表示道路完全被雨水覆盖
            wind_intensity=0.0, # 0 表示平静，100 表示强风，风会影响雨向和树叶
            sun_azimuth_angle=0.0, # 太阳方位角，0～360
            sun_altitude_angle=90.0,  # 太阳高度角，90 是中午，-90 是午夜
            fog_density=0.0, # 0～100 表示雾的浓度或厚度，仅影响RGB相机传感器
            fog_distance=0.0, # 雾开始的距离，单位为米
            wetness=0.0, # 0～100 表示道路湿度百分比，仅影响RGB相机传感器
            fog_falloff=0.0, # 雾的密度，0至无穷大，0 表示雾比空气轻，覆盖整个场景，1表示与空气一样，覆盖正常大小的建筑物
            scattering_intensity=0.0, # 控制光线对雾的穿透程度
            mie_scattering_scale=0.0, # 控制光线与花粉或空气等大颗粒的相互作用，导致天气朦胧，光源周围有光晕，0表示无影响
            rayleigh_scattering_scale=0.0331, # 控制光与空气分子等小粒子的相互作用，取决于光波长，导致白天蓝天或晚上红天
            )
        world.set_weather(weather)
        
        
        
        # =============================创建kafka消费者=================================
        topic = 'tess_send_carla_shangdi'
        bootstrap_servers = '106.120.201.126:14576'
        # 创建Kafka消费者实例
        consumer = KafkaConsumer(
            topic,
            bootstrap_servers = bootstrap_servers,
            value_deserializer=lambda x: json.loads(x.decode('utf-8'))  # 指定反序列化函数为JSON解析
        )    
        
        # =============================创建交通管理器对象，并设置仿真的倍数和车辆最大的速度=================================  
    
        traffic_manager = client.get_trafficmanager(8000)
        traffic_manager.set_global_distance_to_leading_vehicle(2.0)
        traffic_manager.global_percentage_speed_difference(40.0)
        
        if True:
            #设置TM同步模式
            settings = world.get_settings()
            traffic_manager.set_synchronous_mode(True)
            if not  settings.synchronous_mode:
                print("开启同步模式")
                synchronous_master = True
                settings.synchronous_mode = True
                settings.fixed_delta_seconds = 0.1
                world.apply_settings(settings)
            else:
                print("开启异步模式")
                synchronous_master = False
                traffic_manager.set_synchronous_mode(False)
                settings.synchronous_mode = False
                settings.fixed_delta_seconds = 0.1
                world.apply_settings(settings)
        
    # =============================视角坐标=================================
        x, y = rule(116.29220138718945, 40.05073628413568)



        vehicle_trans11 = Transform(Location(x=424.81892146460007, y=-356.75291758771124, z=65.677723), 
                                    Rotation(yaw = 270))
        world.get_spectator().set_transform(vehicle_trans11)  # 设置世界视角
        
        blueprint_library = world.get_blueprint_library()
        
       

   # =======================================雷达==================================

        lidar_x, lidar_y = rule(116.29156843270846,40.05293308929916)

        vehicle_trans_lidar = Transform(Location(x=lidar_x, y=-lidar_y, z=45.677723), 
                                    Rotation(yaw = 347.65-90))
        


        lidar_bp_2_1 = blueprint_library.find('sensor.lidar.ray_cast')  # 雷达
        lidar_bp_2_2 = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_2_3 = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_1 = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_2 = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_3 = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_1 = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_2 = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_3 = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_28 = blueprint_library.find('sensor.lidar.ray_cast')

        # 设置传感器的参数
        atmosphere_attenuation_rate = '0.004'
        dropoff_general_rate = '0.0'
        dropoff_intensity_limit = '0.0'
        dropoff_zero_intensity = '0.0'
        noise_seed = '0.0'
        noise_stddev = '0.0'


        lidar_bp_2_1.set_attribute('channels', '2')
        lidar_bp_2_1.set_attribute('upper_fov', '-1.5')
        lidar_bp_2_1.set_attribute('lower_fov', '-1.7')
        lidar_bp_2_1.set_attribute('points_per_second', '36000')
        lidar_bp_2_1.set_attribute('range', '200')
        lidar_bp_2_1.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_2_1.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_2_1.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_2_1.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_2_1.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_2_1.set_attribute('noise_seed', noise_seed)
        lidar_bp_2_1.set_attribute('noise_stddev', noise_stddev)

        lidar_bp_28.set_attribute('channels', '28')
        lidar_bp_28.set_attribute('upper_fov', '-1.9')
        lidar_bp_28.set_attribute('lower_fov', '-4.6')
        lidar_bp_28.set_attribute('points_per_second', '504000')
        lidar_bp_28.set_attribute('range', '200')
        lidar_bp_28.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_28.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_28.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_28.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_28.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_28.set_attribute('noise_seed', noise_seed)
        lidar_bp_28.set_attribute('noise_stddev', noise_stddev)

        lidar_bp_6_1.set_attribute('channels', '6')
        lidar_bp_6_1.set_attribute('upper_fov', '-4.8')
        lidar_bp_6_1.set_attribute('lower_fov', '-5.8')
        lidar_bp_6_1.set_attribute('points_per_second', '108000')
        lidar_bp_6_1.set_attribute('range', '200')
        lidar_bp_6_1.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_6_1.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_6_1.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_6_1.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_6_1.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_6_1.set_attribute('noise_seed', noise_seed)
        lidar_bp_6_1.set_attribute('noise_stddev', noise_stddev)

        lidar_bp_4_1.set_attribute('channels', '4')
        lidar_bp_4_1.set_attribute('upper_fov', '-6.1')
        lidar_bp_4_1.set_attribute('lower_fov', '-7')
        lidar_bp_4_1.set_attribute('points_per_second', '72000')
        lidar_bp_4_1.set_attribute('range', '200')
        lidar_bp_4_1.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_4_1.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_4_1.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_4_1.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_4_1.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_4_1.set_attribute('noise_seed', noise_seed)
        lidar_bp_4_1.set_attribute('noise_stddev', noise_stddev)
        
        lidar_bp_4_2.set_attribute('channels', '4')
        lidar_bp_4_2.set_attribute('upper_fov', '-7.4')
        lidar_bp_4_2.set_attribute('lower_fov', '-8.6')
        lidar_bp_4_2.set_attribute('points_per_second', '72000')
        lidar_bp_4_2.set_attribute('range', '200')
        lidar_bp_4_2.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_4_2.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_4_2.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_4_2.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_4_2.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_4_2.set_attribute('noise_seed', noise_seed)
        lidar_bp_4_2.set_attribute('noise_stddev', noise_stddev)

        lidar_bp_4_3.set_attribute('channels', '4')
        lidar_bp_4_3.set_attribute('upper_fov', '-9.2')
        lidar_bp_4_3.set_attribute('lower_fov', '-11')
        lidar_bp_4_3.set_attribute('points_per_second', '72000')
        lidar_bp_4_3.set_attribute('range', '200')
        lidar_bp_4_3.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_4_3.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_4_3.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_4_3.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_4_3.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_4_3.set_attribute('noise_seed', noise_seed)
        lidar_bp_4_3.set_attribute('noise_stddev', noise_stddev)  

        lidar_bp_6_2.set_attribute('channels', '6')
        lidar_bp_6_2.set_attribute('upper_fov', '-12')
        lidar_bp_6_2.set_attribute('lower_fov', '-17')
        lidar_bp_6_2.set_attribute('points_per_second', '108000')
        lidar_bp_6_2.set_attribute('range', '200')
        lidar_bp_6_2.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_6_2.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_6_2.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_6_2.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_6_2.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_6_2.set_attribute('noise_seed', noise_seed)
        lidar_bp_6_2.set_attribute('noise_stddev', noise_stddev)
    
        lidar_bp_6_3.set_attribute('channels', '6')
        lidar_bp_6_3.set_attribute('upper_fov', '-19')
        lidar_bp_6_3.set_attribute('lower_fov', '-29')
        lidar_bp_6_3.set_attribute('points_per_second', '108000')
        lidar_bp_6_3.set_attribute('range', '200')
        lidar_bp_6_3.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_6_3.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_6_3.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_6_3.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_6_3.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_6_3.set_attribute('noise_seed', noise_seed)
        lidar_bp_6_3.set_attribute('noise_stddev', noise_stddev)

        lidar_bp_2_2.set_attribute('channels', '2')
        lidar_bp_2_2.set_attribute('upper_fov', '-32')
        lidar_bp_2_2.set_attribute('lower_fov', '-35')
        lidar_bp_2_2.set_attribute('points_per_second', '36000')
        lidar_bp_2_2.set_attribute('range', '200')
        lidar_bp_2_2.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_2_2.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_2_2.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_2_2.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_2_2.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_2_2.set_attribute('noise_seed', noise_seed)
        lidar_bp_2_2.set_attribute('noise_stddev', noise_stddev)

        lidar_bp_2_3.set_attribute('channels', '2')
        lidar_bp_2_3.set_attribute('upper_fov', '-38')
        lidar_bp_2_3.set_attribute('lower_fov', '-42')
        lidar_bp_2_3.set_attribute('points_per_second', '36000')
        lidar_bp_2_3.set_attribute('range', '200')
        lidar_bp_2_3.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_2_3.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_2_3.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_2_3.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_2_3.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_2_3.set_attribute('noise_seed', noise_seed)
        lidar_bp_2_3.set_attribute('noise_stddev', noise_stddev)


        lidar_2_1 = world.spawn_actor(lidar_bp_2_1, vehicle_trans_lidar, attach_to=None)
        lidar_2_2 = world.spawn_actor(lidar_bp_2_2, vehicle_trans_lidar, attach_to=None)
        lidar_2_3 = world.spawn_actor(lidar_bp_2_3, vehicle_trans_lidar, attach_to=None)
        lidar_4_1 = world.spawn_actor(lidar_bp_4_1, vehicle_trans_lidar, attach_to=None)
        lidar_4_2 = world.spawn_actor(lidar_bp_4_2, vehicle_trans_lidar, attach_to=None)
        lidar_4_3 = world.spawn_actor(lidar_bp_4_3, vehicle_trans_lidar, attach_to=None)
        lidar_6_1 = world.spawn_actor(lidar_bp_6_1, vehicle_trans_lidar, attach_to=None)
        lidar_6_2 = world.spawn_actor(lidar_bp_6_2, vehicle_trans_lidar, attach_to=None)
        lidar_6_3 = world.spawn_actor(lidar_bp_6_3, vehicle_trans_lidar, attach_to=None)
        lidar_28 = world.spawn_actor(lidar_bp_28, vehicle_trans_lidar, attach_to=None)

        lidar_2_1.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_2_1"))
        lidar_2_2.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_2_2"))
        lidar_2_3.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_2_3"))
        lidar_4_1.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_4_1"))
        lidar_4_2.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_4_2"))
        lidar_4_3.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_4_3"))
        lidar_6_1.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_6_1"))
        lidar_6_2.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_6_2"))
        lidar_6_3.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_6_3"))
        lidar_28.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_28"))

        sensor_list.append(lidar_2_1)
        sensor_list.append(lidar_2_2)
        sensor_list.append(lidar_2_3)
        sensor_list.append(lidar_4_1)
        sensor_list.append(lidar_4_2)
        sensor_list.append(lidar_4_3)
        sensor_list.append(lidar_6_1)
        sensor_list.append(lidar_6_2)
        sensor_list.append(lidar_6_3)
        sensor_list.append(lidar_28)
               
        if SHOW:

            point_cloud = o3d.geometry.PointCloud()
            vis = o3d.visualization.Visualizer()
            vis.create_window(window_name='Wanji 64line Lidar', width=600, height=500, left=0, top=500, visible=True)

            ctr = vis.get_view_control()
            vis.add_geometry(point_cloud)

            extrinsic = np.eye(4)
            W = 600
            H = 500 
            camera_angle_x = 0.8457078337669373
            focal = 0.5 * W / np.tan(0.5 * camera_angle_x)
            # cx=(W-1)/2而不是W/2 !!!!!
            intrinsic = o3d.cuda.pybind.camera.PinholeCameraIntrinsic(width=W, height=H, fx=focal, fy=focal, cx=(W-1) / 2,cy=(H-1) / 2)
            param = o3d.cuda.pybind.camera.PinholeCameraParameters()
            param.extrinsic = extrinsic
            param.intrinsic = intrinsic
            ctr.convert_from_pinhole_camera_parameters(param)
            
        
            render_option = vis.get_render_option()
            render_option.point_size = 1.0
            render_option.background_color = np.asarray([0, 0, 0])  # 颜色 0为黑；1为白
            to_reset_view_point = True      
        # 持续消费消息
        while True:

            splicing = []

            world.tick()
            p_timestamp = world.get_snapshot().timestamp.platform_timestamp
            w_timestamp = get_time_stamp(p_timestamp)
           
  
            # 解析Kafka消息中的车辆位置信息
            messages = consumer.poll(timeout_ms=100)
        
            for paration in messages.values():
                for message in paration:
                        participants = message.value["participants"]
                        if len(participants) != 0:
                            errorids = []
                            nowids = []                       
                            #添加交通流
                            for par in participants:
                                nowids.append(par["ID"])

                                x, y = rule(par["longitude"], par["latitude"])
                                
                                trans = Transform(carla.Location(x=x, y=-y, z=37),
                                                    carla.Rotation(yaw= par["courseAngle"]-90))
                                
                                print(par["courseAngle"])
                                vehtype = par['type']
                                vehtype = 'vehicle.lincoln.mkz_2017'
                                id = par['ID']
                                
                                if id not in spawed_ids.keys():
                                    bp1 = world.get_blueprint_library().find(vehtype)
                                    try:
                                        bp1.set_attribute('color', '0,0,0')
                                    except:
                                        pass
                                    else:
                                        pass

                                    if vehtype == 'vehicle.lincoln.mkz_2017':
                                        batch = [
                                            carla.command.SpawnActor(bp1, trans).then(
                                                carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
                                        ]
                                        response = client.apply_batch_sync(batch, False)[0]
                                        if response.error:
                                            errorids.append(id)
                                        else:
                                            spawed_ids[id] = response.actor_id
                                    else:
                                        batch = [
                                            carla.command.SpawnActor(bp1, trans).then(
                                                carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
                                        ]
                                        response = client.apply_batch_sync(batch, False)[0]
                                        if response.error:
                                            errorids.append(id)
                                        else:
                                            spawed_ids[id] = response.actor_id
                    
                                else:
                                    if vehtype == 'vehicle.lincoln.mkz_2017':
                                        cid = spawed_ids[id]
                                        cactor = world.get_actor(cid)
                                        cactor.set_transform(trans)
                                    else:
                                        carlaid = spawed_ids[id]
                                        vehicle = world.get_actor(carlaid)
                                        if vehicle is not None:
                                            vehicle.set_transform(trans)
                            print("已有：", len(spawed_ids))

                            # 删除车辆
                            destodyed_ids = [id for id in spawed_ids if id not in nowids]
                            for did in destodyed_ids:
                                carlaid = spawed_ids[did]
                                vehicle = world.get_actor(carlaid)

                                if vehicle is not None:
                                    vehicle.destroy()
                                del spawed_ids[did]
                            print("更新：", len(nowids), "销毁:", len(destodyed_ids))

            # 将所有雷达的数据合并到一个点云文集中
            for i in range(0, len(sensor_list)): 
                s_frame, s_timestamp, s_name, s_data = sensor_queue.get(True, 1.0)
                splicing.append(_parse_lidar_cb(s_data))
                # print(i)

            if splicing:
                concat_points = np.concatenate(splicing, axis=0)
                concat_points[:, 1] = [-p for p in concat_points[:, 1]] #  将生成的点云Y值进行反转 由于carla的坐标系中Y值是反的
                if SAVE_PCD:
                    pcd_path = save_path + "/" + str(w_timestamp) + ".pcd"
                    points2pcd(pcd_path, concat_points)
                if SHOW:
                    points1 = np.array(concat_points)[:, :3]
                    colors1 = np.array(concat_points)[:, 3]
                    
                    point_cloud.points = o3d.utility.Vector3dVector(points1)
                    points_intensity = np.array(concat_points)[:, 3]  # intensity 
                    # print(points_intensity)
                    # r = 0
                    # b = 0
                    # g = 0
                    colors = []
                    for num in points_intensity:
                        # print(num)
                        intensity = num * 255
                        if intensity <= 33:
                            r = 0
                            b = 255
                            g = int(7.727 * intensity)
                        elif intensity >33 and intensity <= 66:
                            r = 0
                            b = int(255 - 7.727 * (intensity - 34))
                            g = 255
                        elif intensity > 66 and intensity <= 100:
                            r = int(7.727 * (intensity - 67))
                            b = 0
                            g = 255
                        elif  intensity > 100 and intensity <= 255:
                            r = 255
                            b = 0
                            g = int(255 - 7.727 * (intensity - 100)/4.697)
                        points_color = [r, g, b]
                        # points_color = [0, 0, 0.8]
                        colors.append(points_color)
                    # print(colors)
                    # 根据 intensity 为点云着色，由于变化不大，这里设置为蓝色[0,0,0.8]
                    point_cloud.colors = o3d.utility.Vector3dVector(colors)   
                    render_opt = vis.get_render_option()
                    render_opt.point_size = 1
                    render_opt.background_color = [0,0, 0]

                    box = o3d.geometry.AxisAlignedBoundingBox([-60,-60,-10],[60,60,20])
                    pcd_box = point_cloud.crop(box)
                    vis.update_geometry(point_cloud)
                    if to_reset_view_point:
                        vis.reset_view_point(True)
                        to_reset_view_point = False
                    vis.poll_events()
                    vis.update_renderer()


          

    finally:


        if synchronous_master:
            settings = world.get_settings()
            settings.synchronous_mode = False
            settings.fixed_delta_seconds = None
            world.apply_settings(settings)
        else:
            settings = world.get_settings()
            settings.synchronous_mode = False
            settings.fixed_delta_seconds = None
            world.apply_settings(settings)

        cv2.destroyAllWindows()
        for sensor in sensor_list:
            sensor.destroy()
        for actor in actor_list:
            if actor.is_alive:
                actor.destroy()
                

                
                    
     

if __name__ == "__main__":
    

    spawed_ids = {}
    actor_list, sensor_list = [], []
    save_path = '/home/tongjipc/carla/Dist/SDWR_UE4/PythonAPI/data'
    SHOW = True # 可视化展示雷达点云
    SAVE_PCD = False # 是否保存点云数据
    sensor_queue = Queue()
    
    try:
        main()
    except KeyboardInterrupt:
        print(' - Exited by user.')
